#include "util.h"

#include <tiros/tiros.h>

  


#define STKSIZE     (TIROS_MIN_CTXT_SZ + 64)
#define NUM_TASKS	     4

/* An array of counting semaphores to signal the tasks. This is just
 * to signal a job to run and is unrelated to the point we are
 * trying to make here.*/
csem_t cs[NUM_TASKS-1];  
mutex_t m1, m2;

/* Stacks for each of our tasks */
osword_t task_stack[NUM_TASKS][STKSIZE];

void wait(void)
{
	delayms(1000);
}

void print_index(void)
{

	int i;
	putstring("R\tRunning - HP\n");
	putstring("P\tAchieved a P() on CS - HP\n");
	putstring("L\tLocked mutex - HP\n");
	putstring("U\tUnlocked mutex - HP\n");
	putstring("r\tRunning - LP\n");
	putstring("l\tLocked mutex - LP\n");
	putstring("u\tUnlocked mutex - LP\n");
	putstring("v\tDid a V() on CS - LP\n");
	putstring("0-4\t Before and after V on CS - LP\n");
	putstring("Task\tPrio\tElev\tMsg\n");
	for(i=50;i;i--)
		putchar('-');
	putchar('\n');

	
}

void printstatus(unsigned char ch)
{
	tid_t tid;
	tid_t bp;
	tid_t ep;
	tid = os_self_tid();
	bp = os_prio_get(tid, 0);
	ep = os_prio_get(tid, O_EFFECTIVE_PRIO);

	putchar('T');
	puthexchar(tid);
	putchar('\t');
	puthexchar(bp);
	putchar('\t');
	puthexchar(ep);
	putchar('\t');
	putchar(ch);
	putchar('\n');

}


void J3(void * dummy) 
{
	int16_t rc;
	while(1) {
		wait();
		/* Lock the mutex M1 */
		printstatus('r');
		rc = mutex_lock(&m1, 0, 0);
		printstatus('l');

		if (rc != 0)
			putstring("J3 lock error");

		wait();
		/* Signal J2 to run */
		putstring("J3 signaling J2 to run\n");
		rc = csem_V(&cs[2]);
		printstatus('v'); 


		/* Signal J0 to run */
		putstring("J3 signaling J0 to run\n");
		rc = csem_V(&cs[0]);
		printstatus('v'); 

		/* Signal J1 to run */
		putstring("J3 signaling J1 to run\n");
		rc = csem_V(&cs[1]);
		printstatus('v'); 
		
		wait();
		wait();
		
		/* Release M1 */
		putstring("J3 unlocking M1\n");
		rc = mutex_unlock(&m1);

		if (rc != SUCCESS)
			putstring("J3 unlock error\n");

	}

}

void J2(void* dummy)
{
	int16_t rc;

	while(1) {
		putstring("J2 waiting for signal\n");

		/* Wait for a signal */
		rc = csem_P(&cs[2], 0, 0);
		putstring("J2 got signal\n");
		printstatus('P');

		if (rc < 0)
			putstring("J2 P error");

		wait();

		/* We have been signaled to wake.  Attempt a mutex
		lock. */
		putstring("J2 locking m2\n");
		rc = mutex_lock(&m2, 0, 0);
		if (rc != SUCCESS)
			putstring("J2 lock error");

		/* Acquired lock */
		printstatus('L');
		wait();
		
		
		putstring("J2 locking m1\n");
		rc = mutex_lock(&m1, 0, 0);
		if (rc != SUCCESS)
			putstring("J2 lock error");
		printstatus('L');
		wait();
		

		putstring("J2 unlocking m1\n");
		rc = mutex_unlock(&m1);
		if (rc != SUCCESS)
			putstring("HP unlock error");
		printstatus('U');
		wait();

		putstring("J2 unlocking m2\n");		
		rc = mutex_unlock(&m2);
		printstatus('U');
		if (rc != SUCCESS)
			putstring("J2 unlock error");
		wait();
	}
	
}

void J1(void* dummy)
{
	int16_t rc;
	while(1) {
		wait();
		putstring("J1 waiting for signal\n");
		rc = csem_P(&cs[1], 0, 0);
		putstring("J1 got signal: running\n");

		wait();
		wait();
		wait();
	}

}


void J0(void* dummy)
{
	int16_t rc;

	while(1) {
		putstring("J0 waiting for signal\n");
		/* Wait for a signal */
		rc = csem_P(&cs[0], 0, 0);
		putstring("J0 got signal\n");
		printstatus('P');

		if (rc != SUCCESS)
			putstring("J0 P error");


		/* We have been signaled to wake.  Attempt a mutex lock. */
		putstring("J0 locking M2\n");
		rc = mutex_lock(&m2, 0, 0);
		if (rc != 0)
			putstring("J0 lock error");
		printstatus('L');
		wait();


		putstring("J0 unlocking M2\n");
		rc = mutex_unlock(&m2);
		printstatus('U');
		if (rc != SUCCESS) {
			putstring("J0 unlock error");
			puthexshort(rc); putchar('\n');
		}
		wait();

	}

}



void platform_init(void);
int main(void) 
{
	int i;
	tid_t J0tid, J1tid, J2tid, J3tid;
	platform_init();

	// --------------------------------------------------
	os_init();

	/* We will allocate the priorities as follows: 
	 * Mutex M2 priority ceiling: 0
	 * J0 : 1
	 * J1 : 2
	 * Mutex M1 priority ceiling: 3
	 * J2 : 4
	 * J3 : 5 */
	mutex_init(&m1, 3);
	mutex_init(&m2, 0);

	for (i=0; i< NUM_TASKS-1; i++) 
		csem_init(&cs[i], 0, TIROS_CSEMVAL_MAX);


	J0tid = os_task_create(J0, (osptr_t)0,
			       task_stack[0], STKSIZE, 1); 

	J1tid = os_task_create(J1, (osptr_t)0,
			       task_stack[1], STKSIZE, 2); 

	J2tid = os_task_create(J2, (osptr_t)0,
			       task_stack[2], STKSIZE, 4); 

	J3tid = os_task_create(J3, (osptr_t)0,
			       task_stack[3], STKSIZE, 5); 

	if (J0tid == ILLEGAL_ELEM || J1tid == ILLEGAL_ELEM ||
	    J2tid == ILLEGAL_ELEM || J3tid == ILLEGAL_ELEM) {
			putstring("Task create failed");
	}

	print_index();
	os_start();
	putstring("Started OS: Shouldn't reach here\n");
	while (1);
}

